

--[[=============================================================================
	SCAT226T倾角传感器解码程序,按照AIr724修改(AIr105的见早期的版本,pack和位操作指令不一样)
	1. 串口收到倾角传感器的数据,发布 publish("Inclinator_Rx", rx)
	2. 本程序订阅"Inclinator_Rx"，并启动倾角数据解码
	3. 解码成功,发布倾角传感器角度数据: sys.publish("Inclinator_Data", "SCA116T",x,y,z,t )
	==============================================================================]]

local sys = require 'sys'


SCA116T = {
	UART_id 	=2 ,		-- 倾角传感器在串口2
	subscribe 	="Inclinator_Rx",		-- 监听串口发送的倾角数据
	publish		= "Inclinator_Data",	-- 发送倾角传感器数据

	timer	= 0,		-- 秒计时++
	x 		= 0,		--倾角数值
	x2 		= 0 		--滤波角度

}




-----------------------------------------------
--   SCA116T解码, 输入倾角传感器数据; 返回: 角度X
-----------------------------------------------
function SCA116T_decode(s)
	local x
	--寻找指令开始标志0x68 0d 00 84，返回开始和结束位置p1,p2

	local p1, p2 = string.find(s, "\x68\x0d\x00\x84") 
	if p1 == nil then
		return nil
	end	

	local s2 = string.sub(s,p2+1)

	if(string.len( s2)) <10 then
		log.error("SCA116T","倾角传感器数据过少异常!")
		return nil
	end

	-- 从p2位置开始读3个字节BCD数据(ah,al),(bh,bl),(ch,cl): 角度值= al/bh/bl . ch/cl ;  ah 是负号标志
	local n,a,b,c = pack.unpack(s2, "bbb")
	--log.debug("SCA116T", "n,a,b,c=", n,a,b,c)	

	local x= bit.band(a,0x0f) * 100 + bit.rshift(b,4)*10 + bit.band(b,0x0f) +  bit.rshift(c,4)/10 + bit.band(c,0x0f)/100
		
	if a>0x0f then		--ah 是负号标志
		x = -x
	end

	x = x+90
	return x
end



------------------------------------------------------
--    监听串口数据,并解码,发布
--      订阅: Inclinator_Rx, 收到倾角模块发送的数据rx,
-- 		解析SCA116T数据 rx,计时清零
--		发布角度数据
------------------------------------------------------

local cnt =0
sys.subscribe( "Inclinator_Rx", function(rx) 
	local x = SCA116T_decode(rx)
	
	if x then	-- SCA116T解码成功
		
		if cnt ==0 then
			TurnOn_Red_LED()
			cnt =1
		else
			TurnOff_Red_LED()
			cnt =0
		end

		SCA116T.timer =0
		SCA116T.x = x

		sys.publish("Inclinator_Data", "SCA116T",x )		--发布消息“DipSensorData”，传感器型号=SCA16T，X，Y，Z
		-- log.debug("SCA116T","解码成功，角度：",x)
	end	

end ) 



-- 倾角传感器类型是SCA116T? 上电2秒钟后发送SCA116T初始化指令
-- 0x68, 0x05, 0, 0x0c,0x01,0x12   (01) 5Hz 自动输出模式
-- 0x68, 0x05, 0, 0x0c,0x02,0x13   (02) 15Hz 自动输出模式
sys.timerStart(function()
    uart.write(SCA116T.UART_id,  string.char(0x68, 0x05, 0, 0x0c,0x01,0x12))
    log.info( "SCA116T","向SCA116T发送初始化数据:", "0x68, 0x05, 0, 0x0c,0x01,0x12" ) 
end, 2000)

-- 每100ms对SCA116T做计时和滤波
sys.timerLoopStart(function()
	if SCA116T.timer  < 9999 then
		SCA116T.timer =  SCA116T.timer +0.1
	end

	if (SCA116T.x -SCA116T.x2) >5 or (SCA116T.x -SCA116T.x2) <-5 then
		SCA116T.x2 = SCA116T.x2 + (SCA116T.x-SCA116T.x2)/16	--快速滤波
	else
		SCA116T.x2 = SCA116T.x2 + (SCA116T.x-SCA116T.x2)/128	--滤波
	end

end, 
100)      --每0.1秒